# Untitled - By: lenovo - Thu Jul 25 2024

import sensor
import image
from pyb import UART


# sensor 初始化
sensor.reset()
# sensor初始配置
sensor.set_pixformat(sensor.RGB565)
# sensor.set_framesize(sensor.QVGA)
# 只有在QQVGA的设置下,才可以使用矩形识别 160*120
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames()
# 颜色捕捉时,设置为False
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)


# 创建串口通信对象
uart = UART(3, 115200, bits=8, parity=None, stop=1)
# 创建image对象
img = sensor.snapshot()
# 创建检测区间
# 该框原点坐标
findLen = [85, 85]
subXY = [(img.width() // 2) - (findLen[0] // 2), (img.height() // 2) - (findLen[1] // 2)]
scope = [subXY[0], subXY[1], findLen[0], findLen[1]]
# 存放识别到的矩形数据
rectangleData = []


# 找到最大的矩形
def FindMaxRectangle(rects):
    maxSize = 0
    ret = -1
    for recognize in rects:
        print(recognize.magnitude())
        if maxSize < recognize.magnitude():
            maxSize = recognize.magnitude()
            ret = recognize
    return ret


# 绘制识别到的矩形
def DrawRecognizeRectangle(recognize):
    global img
    img.draw_rectangle(recognize.rect())
#    for XY in recognize.corners():
#        img.draw_circle(XY[0], XY[1], 5, color=(0, 255, 0))
    XY = recognize.corners()
    img.draw_circle(XY[0][0], XY[0][1], 5, color=(255, 0, 0))
    img.draw_circle(XY[1][0], XY[1][1], 5, color=(0, 255, 0))
    img.draw_circle(XY[2][0], XY[2][1], 5, color=(0, 0, 255))
    img.draw_circle(XY[3][0], XY[3][1], 5, color=(255, 255, 255))


# 将检测到的数据进行留存
def LoadRecognizeData(recognize):
    global rectangleData
    if len(rectangleData) == 50:
        rectangleData.pop(0)
    sure = True
    for XY in recognize.corners():
        if XY[0] < 0 or XY[1] < 0:
            sure = False
    if sure:
        rectangleData.append(recognize)


# 计算方差
def variance(nums):
    if len(nums) == 0:
        return 0
    avg = sum(nums) / len(nums)
    ret = 0
    for i in range(0, len(nums)):
        ret += (nums[i] - avg) ** 2
    return ret / len(nums)

# 对数据进行去噪处理
def DataFilter(nums):
    ret = nums
    ret.sort()
    while variance(ret) > 8:
        ret.pop(0)
        ret.pop(len(ret) - 1)
        if len(ret) == 0:
            break
    return ret

# 对采集到的矩形进行滤波,去除噪点影响
def RectangleFilter():
    global rectangleData
    ret = [[0, 0], [0, 0], [0, 0], [0, 0]]
    if len(rectangleData) > 30:
        for i in range(0, 4):
            retx = []
            rety = []
            for elem in rectangleData:
                XY = elem.corners()
                retx.append(XY[i][0])
                rety.append(XY[i][1])
            retx = DataFilter(retx)
            rety = DataFilter(rety)
            if len(retx) == 0 or len(rety) == 0:
                return [[0, 0], [0, 0], [0, 0], [0, 0]]
            ret[i] = [sum(retx) // len(retx), sum(rety) // len(rety)]
    return ret


# 发送坐标数据
def UARTSendRectangleXY(rectangleXY):
    global uart
#    发送原始坐标数据
    for XY in rectangleXY.corners():
#        从左上角开始顺时针返回坐标
        sendarray = str(XY[0]) + ' ' + str(XY[1])
        uart.write(sendarray)
#        print(sendarray)
    print('')


# 运行开始处
while True:
    img = sensor.snapshot()
#    去畸变
    img.lens_corr(strength=1.8, zoom=1.0)
    img.draw_rectangle(scope, thickness=1)

    # 接收找到的最大矩形
    recognize = FindMaxRectangle(img.find_rects(threshold=7500, roi=scope))
    # 找不到时,返回-1需要处理一下
    if recognize == -1:
        continue

    # 将找到的矩形进行标记
    DrawRecognizeRectangle(recognize)

    # 发送矩形坐标数据
    UARTSendRectangleXY(recognize)
